#include "mechanical_arm_behaviour.h"

/**
  * @brief          ????????????????????????????????????????h???0??
  * @param          ???????????
  * @param          ?????????????????????
  * @param          ?????
  */
#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }
		
static void mechanical_arm_behavour_set(mechanical_arm_control_t *mechanical_arm_mode_set);
		
mechanical_arm_behaviour_e mechanical_arm_behaviour = MECHANICAL_ARM_ZERO_FORCE;
		
void mechanical_arm_behaviour_mode_set(mechanical_arm_control_t *mechanical_arm_mode_set)
{
    if (mechanical_arm_mode_set == NULL)
    {
        return;
    }
		
    mechanical_arm_behavour_set(mechanical_arm_mode_set);

    if (mechanical_arm_behaviour == MECHANICAL_ARM_ZERO_FORCE)//??????
    {
				
    }

}

//bool_t mechanical_arm_cmd_to_chassis_stop(void)
//{
//    if (mechanical_arm_behaviour ==  || mechanical_arm_behaviour ==  || mechanical_arm_behaviour == MECHANICAL_ARM_ZERO_FORCE)
//    {
//        return 1;
//    }
//    else
//    {
//        return 0;
//    }
//}

void mechanical_arm_behaviour_control_set(fp32 *add_yaw, fp32 *add_pitch, mechanical_arm_control_t *machanical_arm_control_set)
{

    if (add_yaw == NULL || add_pitch == NULL || machanical_arm_control_set == NULL)
    {
        return;
    }
    if (mechanical_arm_behaviour == MECHANICAL_ARM_ZERO_FORCE)
    {
        
    }
}

static void mechanical_arm_behavour_set(mechanical_arm_control_t *mechanical_arm_mode_set)
{
    static mechanical_arm_behaviour_e last_mechanical_arm_behaviour = MECHANICAL_ARM_ZERO_FORCE;
    if (mechanical_arm_mode_set == NULL)
    {
        return;
    }
  
    //??????? ?????
    if (switch_is_down(mechanical_arm_mode_set->gimbal_rc_ctrl->rc.s[0]))//????
    {
        mechanical_arm_behaviour = MECHANICAL_ARM_ZERO_FORCE;
    }
//    else if (switch_is_mid(mechanical_arm_mode_set->gimbal_rc_ctrl->rc.s[0]))//????
//    {
//        mechanical_arm_behaviour = ;
//    }
//    else if (switch_is_up(mechanical_arm_mode_set->gimbal_rc_ctrl->rc.s[0]))//????
//    {
//        mechanical_arm_behaviour = ;
//    }


//    if (last_mechanical_arm_behaviour == MECHANICAL_ARM_ZERO_FORCE && mechanical_arm_behaviour != MECHANICAL_ARM_ZERO_FORCE)
//    {
//				mechanical_arm_behaviour = ;
//    }
    last_mechanical_arm_behaviour = mechanical_arm_behaviour;
}
